#include "Trajectory.h"
#include "Utilities.h"

using namespace utilities;

using std::cout;
using std::endl;
using std::string;
using std::vector;
using std::stringstream;
using std::ifstream;
using std::ofstream;

_TRAJECTORY_BEGIN_

Trajectory::Trajectory()
{

}

Trajectory::Trajectory(const Trajectory& obj) 
{ 
	m_Points.clear();
	m_Points = obj.m_Points;
}

Trajectory& Trajectory::operator=(const Trajectory& obj)
{
	m_Points.clear();
	m_Points = obj.m_Points;
	return *this;
}

Trajectory::~Trajectory() 
{

}

/************************************************************************/
/*	The member function of the class Trajectories.                        */
/************************************************************************/


double Trajectory::calculateMDLnopar(int i, int j)
{
	double nopar = 0;

	DataPoint tmpPoint1, tmpPoint2;

	/*	Calculate the MDLnopar	*/
	for (int k = i; k < j - 1; ++k)
	{
		tmpPoint1 = m_Points[k];
		tmpPoint2 = m_Points[k + 1];
		nopar += calculateEuclideanDistance(tmpPoint1[0], tmpPoint1[1], tmpPoint2[0], tmpPoint2[1]);
	}
	nopar = static_cast<double>(log(nopar) / log(2.0));

	return nopar;
}

double Trajectory::calculateMDLpar(int i, int j)
{
	double par = 0;
	double LH = 0;
	double DHper = 0, DHthe = 0;
	DataPoint tmpPoint1 = m_Points[i];
	DataPoint tmpPoint2 = m_Points[j];	/*	Store point j	*/
	DataPoint tmpPoint3;
	DataPoint tmpPoint4;
	LH = static_cast<double>(log(calculateEuclideanDistance(tmpPoint1[0], tmpPoint1[1], tmpPoint2[0], tmpPoint2[1])) / log(2.0));

	for (int k = i; k < j - 1; ++k)
	{
		tmpPoint3 = m_Points[k];
		tmpPoint4 = m_Points[k + 1];
		DHper += calculatePerpendicularDistanceEx(tmpPoint1[0], tmpPoint1[1], tmpPoint2[0], tmpPoint2[1], tmpPoint3[0], tmpPoint3[1], tmpPoint4[0], tmpPoint4[1]);
		DHthe += calculateAngleDistanceEx(tmpPoint1[0], tmpPoint1[1], tmpPoint2[0], tmpPoint2[1], tmpPoint3[0], tmpPoint3[1], tmpPoint4[0], tmpPoint4[1]);
	}

	par = static_cast<double>(LH + log(DHper) / log(2.0) + log(DHthe) / log(2.0));

	return par;
}

/************************************************************************/
/*	Coarse partition the trajectory.                                    */
/************************************************************************/
const vector<int>& Trajectory::coarsePartition()
{
	m_CP.push_back(0);
	int startIndex = 0, length = 1;

	while (startIndex + length < (int)m_Points.size())
	{
		int currentIndex = startIndex + length;
		double costPar = calculateMDLpar(startIndex, currentIndex);
		double costNopar = calculateMDLnopar(startIndex, currentIndex);
		if ((costPar > costNopar) && (currentIndex - 1 != 0))
		{
			m_CP.push_back(currentIndex - 1);
			startIndex = currentIndex - 1;
			length = 1;
		}
		else
		{
			length += 1;
		}
	}
	m_CP.push_back(m_Points.size() - 1);
	//cout << m_Name << ":" << m_CP.size() << endl;
	
	// test
	/*cout << m_Name << ":";
	for (int i = 0; i < (int)m_CP.size(); i++)
	{
		cout << " [" << m_Points[m_CP[i]][0] << "," << m_Points[m_CP[i]][1] << "]";
	}
	cout << endl;*/
	return m_CP;
}

void Trajectory::calculateCoarseParameters()
{
	m_MaxPer.resize(m_CP.size());
	m_MaxLen.resize(m_CP.size());
	m_MinLen.resize(m_CP.size());
	m_MaxThe.resize(m_CP.size());

	for (int i = 0; i < (int)m_CP.size(); ++i)
	{
		int endIndex;
		double maxPer = 0.0;
		double maxLen = 0.0;
		double minLen = 0.0;
		double maxThe = 0.0;
		if (i == (int)m_CP.size() - 1)	//	the last coarse partition
			endIndex = m_Points.size() - 1;
		else
			endIndex = m_CP[i + 1];

		for (int j = m_CP[i]; j < endIndex; ++j)
		{
			double perDist = calculatePerpendicularDistanceEx(m_Points[j][0], m_Points[j][1], m_Points[j + 1][0], m_Points[j + 1][1],
								m_Points[m_CP[i]][0], m_Points[m_CP[i]][1], m_Points[m_CP[i + 1]][0], m_Points[m_CP[i + 1]][1]);
			double len = calculateEuclideanDistance(m_Points[j][0], m_Points[j][1], m_Points[j + 1][0], m_Points[j + 1][1]);
			double the = calculateAngleDistanceEx(m_Points[j][0], m_Points[j][1], m_Points[j + 1][0], m_Points[j + 1][1],
								m_Points[m_CP[i]][0], m_Points[m_CP[i]][1], m_Points[m_CP[i + 1]][0], m_Points[m_CP[i + 1]][1]);
			if (maxPer < perDist)
			{
				maxPer = perDist;
			}
			if (minLen > len)
			{
				minLen = len;
			}
			if (maxLen < len)
			{
				maxLen = len;
			}
			if (maxThe < the)
			{
				maxThe = the;
			}
		}
		m_MaxPer[i] = maxPer;
		m_MaxLen[i] = maxLen;
		m_MinLen[i] = minLen;
		m_MaxThe[i] = maxThe;
	}
	//cout << m_Name << ":" << m_MaxPer.size() << endl;
}

/************************************************************************/
/*	Get the coarse partition parameters.                                */
/*	Parameters:															*/
/*		index  - the index of the partition								*/
/*		maxPer - the maxPer of this partition							*/
/*		maxLen - the maxLen of this partition							*/
/*		minLen - the minLen of this partition							*/
/*		maxThe - the maxThe of this partition							*/
/************************************************************************/
void Trajectory::getCoarseParameters(int index, double& maxPer, double& maxLen, double& minLen, double& maxThe)
{
	maxPer = m_MaxPer[index];
	maxLen = m_MaxLen[index];
	minLen = m_MinLen[index];
	maxThe = m_MaxThe[index];
}



_TRAJECTORY_END_